#include "../User/Device/Control.h"

#include "../User/BSP/gpio.h"
#include "../User/BSP/pwm.h"

void Control_Init() {
  BSP_PWM_Start(BSP_PWM_GetChannelHandle(BSP_PWM_Motor_A));
  BSP_PWM_Start(BSP_PWM_GetChannelHandle(BSP_PWM_Motor_B));
}

void Control_MotorOut(int32_t motor_a, int32_t motor_b) {
  if (motor_a > 0) {
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_A_1, BSP_GPIO_HIGH);
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_A_2, BSP_GPIO_LOW);
  } else if (motor_a < 0) {
    motor_a = -motor_a;
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_A_1, BSP_GPIO_LOW);
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_A_2, BSP_GPIO_HIGH);
  } else {
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_A_1, BSP_GPIO_HIGH);
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_A_2, BSP_GPIO_HIGH);
  }
  if (motor_b > 0) {
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_B_1, BSP_GPIO_HIGH);
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_B_2, BSP_GPIO_LOW);
  } else if (motor_b < 0) {
    motor_b = -motor_b;
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_B_1, BSP_GPIO_LOW);
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_B_2, BSP_GPIO_HIGH);
  } else {
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_B_1, BSP_GPIO_HIGH);
    BSP_GPIO_WritePin(BSP_GPIO_MOTOR_B_2, BSP_GPIO_HIGH);
  }

  BSP_PWM_SetCompare(BSP_PWM_GetChannelHandle(BSP_PWM_Motor_A), motor_a);
  BSP_PWM_SetCompare(BSP_PWM_GetChannelHandle(BSP_PWM_Motor_B), motor_b);
}

void Control_CalOutput(PID_Param_t* angle_pid, PID_Param_t* speed_pid,
                       AHRS_Angle_t* angle, AHRS_Gyro_t* gyro,
                       CMD_Param_t* cmd) {
  int32_t Motor_PID_Apply_a =
      PID_Apply(angle_pid, angle->Pitch, gyro->y, cmd->pitch_setpoint);
  int32_t Motor_PID_Apply_b =
      PID_Apply(angle_pid, angle->Roll, gyro->x, cmd->roll_setpoint);
  Control_MotorOut(Motor_PID_Apply_a, Motor_PID_Apply_b);
}
